// #include "stdio.h"
// #include "string.h"
// #include "stdint.h"
// #include "signal.h"
// #include "unistd.h"
// #include "stdlib.h"

// // SOEM库头文件
// #include "soem/soem.h"
// #include "osal.h"
// #include "oshw.h"
// #include <inttypes.h>

// #define MAX_SLAVES 16
// #define EC_CYCLETIME     2000000
// #define EC_CYCLETIME_US  (EC_CYCLETIME / 1000)
// #define MOTOR_RESOLUTION  131072
// #define CIRCLE_CNT        MOTOR_RESOLUTION
// #define MODE_CSP          0x08

// #pragma pack(push, 1)
// typedef struct {
//     uint16_t controlword;
//     int32_t  target_pos;
//     int32_t  target_vel;
//     int16_t  target_torque;
//     int16_t  max_torque;
//     int8_t   mode_op;
//     int8_t   reserved;
// } outputs_t;

// typedef struct {
//     uint16_t statusword;
//     int32_t  actual_pos;
//     int32_t  actual_vel;
//     int16_t  actual_torque;
//     int16_t  error_code;
//     int8_t   mode_op;
//     int8_t   reserved;
// } inputs_t;
// #pragma pack(pop)

// typedef struct {
//     int slave_index;
//     uint8* outputs;
//     uint8* inputs;
//     int Obytes;
//     int Ibytes;
//     boolean is_operational;
//     int32_t target_pos;
//     int32_t actual_pos;
//     int32_t position_a;
//     int32_t position_b;  
//     int32_t position_c;
//     int cnt_per_cycle;
//     int pos_state;
//     int cycle_cnt;
//     char name[32];
// } motor_config_t;

// ecx_contextt ecx_context;
// uint8_t      IOmap[4096];
// pthread_t    thread_monitor;
// static motor_config_t motor_configs[MAX_SLAVES];
// static int motor_count = 0;
// volatile int  wkc;
// int           expectedWKC;
// boolean       inOP;
// boolean       needlf = FALSE;
// uint8_t       current_group = 0;
// boolean       exit_flag = FALSE;

// void *slave_monitor(void *ptr);
// void csp_control_main(char *ifname);
// void print_usage();
// void signal_handler(int sig);
// void init_motor_configs(void);
// void setup_motor_control_parameters(int motor_idx);
// void control_single_motor(int motor_idx);
// void print_motor_status(int motor_idx);
// boolean validate_motor_data_access(int motor_idx);
// void verify_motor_pdo_mapping(void);

// int main(int argc, char **argv) {
//     signal(SIGINT, signal_handler);
//     printf("===================== EtherCAT Single Motor CSP Control Program =====================\n");
//     printf("Controlling Motor 0 (Slave 1)\n");

//     if (argc != 2) {
//         print_usage();
//         ec_adaptert *adapter = ec_find_adapters();
//         ec_adaptert *adapter_head = adapter;
//         if (adapter != NULL) {
//             printf("\nAvailable Ethernet Adapters:\n");
//             while (adapter != NULL) {
//                 printf("  - %s  (%s)\n", adapter->name, adapter->desc);
//                 adapter = adapter->next;
//             }
//         } else {
//             printf("\nNo available Ethernet Adapters found!\n");
//         }
//         ec_free_adapters(adapter_head);
//         return -1;
//     }

//     if (pthread_create(&thread_monitor, NULL, slave_monitor, NULL) != 0) {
//         printf("ERROR: Failed to create slave monitor thread!\n");
//         return -1;
//     }

//     csp_control_main(argv[1]);

//     inOP = FALSE;
//     pthread_join(thread_monitor, NULL);
//     printf("\n===================== Program Exited Normally =====================\n");
//     return 0;
// }

// void *slave_monitor(void *ptr) {
//     (void)ptr;
//     int slave_idx;

//     while (!exit_flag) {
//         if (inOP && ((wkc < expectedWKC) || ecx_context.grouplist[current_group].docheckstate)) {
//             if (needlf) {
//                 needlf = FALSE;
//                 printf("\n");
//             }

//             ecx_context.grouplist[current_group].docheckstate = FALSE;
//             ecx_readstate(&ecx_context);

//             for (slave_idx = 1; slave_idx <= ecx_context.slavecount; slave_idx++) {
//                 ec_slavet *slave = &ecx_context.slavelist[slave_idx];
//                 if (slave->group != current_group) continue;

//                 if (slave->state != EC_STATE_OPERATIONAL) {
//                     ecx_context.grouplist[current_group].docheckstate = TRUE;

//                     for (int i = 0; i < motor_count; i++) {
//                         if (motor_configs[i].slave_index == slave_idx) {
//                             motor_configs[i].is_operational = FALSE;
//                             break;
//                         }
//                     }

//                     if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
//                         printf("ERROR: Slave %d(Motor %d) is in [SAFE_OP + ERROR]! Trying to ACK...\n", slave_idx,slave_idx-1);
//                         slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
//                         ecx_writestate(&ecx_context, slave_idx);
//                     }
//                     else if (slave->state == EC_STATE_SAFE_OP) {
//                         printf("WARNING: Slave %d(Motor %d) is in [SAFE_OP]! Switching to OP...\n", slave_idx,slave_idx-1);
//                         slave->state = EC_STATE_OPERATIONAL;
//                         ecx_writestate(&ecx_context, slave_idx);
//                     }
//                     else if (slave->state > EC_STATE_NONE) {
//                         if (ecx_reconfig_slave(&ecx_context, slave_idx, EC_TIMEOUTRET)) {
//                             slave->islost = FALSE;
//                             printf("INFO: Slave %d(Motor %d) reconfigured successfully!\n", slave_idx,slave_idx-1);
//                         }
//                     }
//                     else if (!slave->islost) {
//                         ecx_statecheck(&ecx_context, slave_idx, EC_STATE_OPERATIONAL, EC_TIMEOUTRET * 5);
//                         if (slave->state == EC_STATE_NONE) {
//                             slave->islost = TRUE;
//                             printf("ERROR: Slave %d(Motor %d) lost!\n", slave_idx,slave_idx-1);
//                         }
//                     }
//                 }

//                 if (slave->islost) {
//                     if (slave->state == EC_STATE_NONE) {
//                         if (ecx_recover_slave(&ecx_context, slave_idx, EC_TIMEOUTRET)) {
//                             slave->islost = FALSE;
//                             for (int i = 0; i < motor_count; i++) {
//                                 if (motor_configs[i].slave_index == slave_idx) {
//                                     motor_configs[i].is_operational = TRUE;
//                                     break;
//                                 }
//                             }
//                             printf("INFO: Slave %d(Motor %d) recovered successfully!\n", slave_idx,slave_idx-1);
//                         }
//                     } else {
//                         slave->islost = FALSE;
//                          for (int i = 0; i < motor_count; i++) {
//                             if (motor_configs[i].slave_index == slave_idx) {
//                                 motor_configs[i].is_operational = TRUE;
//                                 break;
//                             }
//                         }
//                         printf("INFO: Slave %d(Motor %d) found!\n", slave_idx,slave_idx-1);
//                     }
//                 }
//             }

//             if (!ecx_context.grouplist[current_group].docheckstate) {
//                 printf("INFO: All slaves resumed [OPERATIONAL] state!\n");
//                 for (int i = 0; i < motor_count; i++) {
//                     motor_configs[i].is_operational = TRUE;
//                 }
//             }
//         }

//         osal_usleep(10000);
//     }
//     return NULL;
// }

// boolean validate_motor_data_access(int motor_idx) {
//     if (motor_idx < 0 || motor_idx >= motor_count) {
//         return FALSE;
//     }
    
//     if (motor_configs[motor_idx].outputs == NULL || motor_configs[motor_idx].inputs == NULL) {
//         printf("WARNING: Motor %d has NULL data pointers!\n", motor_idx);
//         return FALSE;
//     }
    
//     if (motor_configs[motor_idx].Obytes < sizeof(outputs_t) || 
//         motor_configs[motor_idx].Ibytes < sizeof(inputs_t)) {
//         printf("WARNING: Motor %d has insufficient data size (O:%d I:%d)\n", 
//                motor_idx, motor_configs[motor_idx].Obytes, motor_configs[motor_idx].Ibytes);
//         return FALSE;
//     }
    
//     return TRUE;
// }

// void init_motor_configs(void) {
//     motor_count = 0;
    
//     for (int i = 1; i <= ecx_context.slavecount && motor_count < MAX_SLAVES; i++) {
//         // 只初始化motor0（从站1）
//         if (i != 1) {
//             continue;
//         }
        
//         motor_configs[motor_count].slave_index = i;
//         motor_configs[motor_count].outputs = ecx_context.slavelist[i].outputs;
//         motor_configs[motor_count].inputs = ecx_context.slavelist[i].inputs;
//         motor_configs[motor_count].Obytes = ecx_context.slavelist[i].Obytes;
//         motor_configs[motor_count].Ibytes = ecx_context.slavelist[i].Ibytes;
//         motor_configs[motor_count].is_operational = FALSE;
        
//         setup_motor_control_parameters(motor_count);
        
//         snprintf(motor_configs[motor_count].name, 32, "Motor-0");
        
//         printf("  Motor 0: Slave %d, Obytes=%d, Ibytes=%d\n", 
//                i, 
//                motor_configs[motor_count].Obytes, 
//                motor_configs[motor_count].Ibytes);
        
//         motor_count++;
//     }
    
//     printf("INFO: Configured %d motors in total (only Motor0 active)\n", motor_count);
// }

// void setup_motor_control_parameters(int motor_idx) {
//     // 设置motor0的运动参数（小范围运动）
//     motor_configs[motor_idx].position_a = -CIRCLE_CNT * 0.5;
//     motor_configs[motor_idx].position_b = CIRCLE_CNT * 0.6;
//     motor_configs[motor_idx].position_c = (motor_configs[motor_idx].position_a + motor_configs[motor_idx].position_b) * 2 / 4;
//     motor_configs[motor_idx].cnt_per_cycle = 50;
    
//     motor_configs[motor_idx].target_pos = 0;
//     motor_configs[motor_idx].actual_pos = 0;
//     motor_configs[motor_idx].pos_state = 0;
//     motor_configs[motor_idx].cycle_cnt = 0;
// }

// void control_single_motor(int motor_idx) {
//     outputs_t *out_ptr = (outputs_t *)motor_configs[motor_idx].outputs;
//     inputs_t  *in_ptr = (inputs_t *)motor_configs[motor_idx].inputs;
    
//     motor_configs[motor_idx].cycle_cnt++;
//     int cycle_cnt = motor_configs[motor_idx].cycle_cnt;
    
//     if (cycle_cnt < 100) {
//         out_ptr->controlword = 0x0006;
//         out_ptr->mode_op = MODE_CSP;
//         out_ptr->max_torque = 1000;
        
//         if (cycle_cnt == 1) {
//             printf("INFO: Motor 0 - Setting mode to CSP (0x%02X)\n", MODE_CSP);
//         }
//     } else if (cycle_cnt < 200) {
//         out_ptr->controlword = 0x0086;
//         out_ptr->mode_op = MODE_CSP;
        
//         if (cycle_cnt == 150 && in_ptr->mode_op != MODE_CSP) {
//             printf("WARNING: Motor 0 mode not set! Current: 0x%02X, Expected: 0x%02X\n", 
//                    in_ptr->mode_op, MODE_CSP);
//         }
//     } else if (cycle_cnt < 300) {
//         out_ptr->controlword = 0x0007;
//         out_ptr->mode_op = MODE_CSP;
//         out_ptr->target_pos = in_ptr->actual_pos;
//         motor_configs[motor_idx].target_pos = in_ptr->actual_pos;
        
//         if (cycle_cnt == 250 && in_ptr->mode_op != MODE_CSP) {
//             printf("ERROR: Motor 0 failed to set CSP mode! Current: 0x%02X\n", in_ptr->mode_op);
//         }
//     } else if (cycle_cnt < 400) {
//         out_ptr->controlword = 0x000F;
//         out_ptr->mode_op = MODE_CSP;
//         out_ptr->target_pos = motor_configs[motor_idx].target_pos;
//     } else if (cycle_cnt < 500) {
//         out_ptr->controlword = 0x001F;
//         out_ptr->mode_op = MODE_CSP;
//         out_ptr->target_pos = motor_configs[motor_idx].target_pos;
        
//         if (cycle_cnt == 450) {
//             if (in_ptr->mode_op == MODE_CSP) {
//                 printf("SUCCESS: Motor 0 CSP mode activated!\n");
//             } else {
//                 printf("ERROR: Motor 0 CSP mode failed! Current mode: 0x%02X\n", in_ptr->mode_op);
//             }
//         }
//     } else {
//         out_ptr->mode_op = MODE_CSP;
        
//         switch (motor_configs[motor_idx].pos_state) {
//             case 0:
//                 if (motor_configs[motor_idx].target_pos > motor_configs[motor_idx].position_a) {
//                     motor_configs[motor_idx].target_pos -= motor_configs[motor_idx].cnt_per_cycle;
//                 } else if (motor_configs[motor_idx].target_pos < motor_configs[motor_idx].position_a) {
//                     motor_configs[motor_idx].target_pos += motor_configs[motor_idx].cnt_per_cycle;
//                 } else {
//                     motor_configs[motor_idx].pos_state = 1;
//                 }
//                 if (abs(motor_configs[motor_idx].target_pos - motor_configs[motor_idx].position_a) < motor_configs[motor_idx].cnt_per_cycle) {
//                     motor_configs[motor_idx].target_pos = motor_configs[motor_idx].position_a;
//                 }
//                 break;

//             case 1:
//                 if (motor_configs[motor_idx].target_pos < motor_configs[motor_idx].position_c) {
//                     motor_configs[motor_idx].target_pos += motor_configs[motor_idx].cnt_per_cycle;
//                 } else if (motor_configs[motor_idx].target_pos > motor_configs[motor_idx].position_c) {
//                     motor_configs[motor_idx].target_pos -= motor_configs[motor_idx].cnt_per_cycle;
//                 } else {
//                     motor_configs[motor_idx].pos_state = 0;
//                 }
//                 if (abs(motor_configs[motor_idx].target_pos - motor_configs[motor_idx].position_c) < motor_configs[motor_idx].cnt_per_cycle) {
//                     motor_configs[motor_idx].target_pos = motor_configs[motor_idx].position_c;
//                 }
//                 break;

//             default:
//                 motor_configs[motor_idx].pos_state = 0;
//                 break;
//         }

//         out_ptr->target_pos = motor_configs[motor_idx].target_pos;
//         out_ptr->controlword = 0x000F;
//     }
    
//     motor_configs[motor_idx].actual_pos = in_ptr->actual_pos;
// }

// void verify_motor_pdo_mapping(void) {
//     printf("Step 4.1: Verifying PDO Mapping for Motor0...\n");
    
//     for (int i = 0; i < motor_count; i++) {
//         int slave_idx = motor_configs[i].slave_index;
//         ec_slavet *slave = &ecx_context.slavelist[slave_idx];
        
//         printf("  Motor 0 (Slave %d):\n", slave_idx);
//         printf("    - Output bytes: %d (required: %lu)\n", 
//                motor_configs[i].Obytes, sizeof(outputs_t));
//         printf("    - Input bytes: %d (required: %lu)\n", 
//                motor_configs[i].Ibytes, sizeof(inputs_t));
        
//         if (motor_configs[i].Obytes < sizeof(outputs_t)) {
//             printf("    WARNING: Output PDO mapping may be incomplete!\n");
//         }
//         if (motor_configs[i].Ibytes < sizeof(inputs_t)) {
//             printf("    WARNING: Input PDO mapping may be incomplete!\n");
//         }
        
//         printf("    - Name: %s\n", slave->name);
//         printf("    - State: 0x%02X\n", slave->state);
//     }
// }

// void print_motor_status(int motor_idx) {
//     if (!validate_motor_data_access(motor_idx)) {
//         printf("M0: INVALID_DATA | ");
//         return;
//     }
    
//     inputs_t *in_ptr = (inputs_t *)motor_configs[motor_idx].inputs;
//     outputs_t *out_ptr = (outputs_t *)motor_configs[motor_idx].outputs;
    
//     printf("M0: Tgt=%8d Act=%8d OutMode=0x%02X InMode=0x%02X Stat=0x%04X | ", 
//            motor_configs[motor_idx].target_pos,
//            motor_configs[motor_idx].actual_pos,
//            out_ptr->mode_op,
//            in_ptr->mode_op,
//            in_ptr->statusword);
// }

// void csp_control_main(char *ifname) {
//     int i, chk;
    
//     printf("\nStep 1: Initialize EtherCAT Master on %s...\n", ifname);
//     if (!ecx_init(&ecx_context, ifname)) {
//         printf("ERROR: Failed to initialize EtherCAT Master! (Run as root?)\n");
//         goto cleanup;
//     }

//     printf("Step 2: Configure Slaves...\n");
//     int slave_count = ecx_config_init(&ecx_context);
//     if (slave_count <= 0) {
//         printf("ERROR: No EtherCAT Slaves found on the bus!\n");
//         goto cleanup;
//     }
//     printf("INFO: Found %d EtherCAT Slave(s)!\n", slave_count);

//     printf("Step 3: Map PDOs...\n");
//     ecx_context.manualstatechange = 1;

//     ecx_context.slavelist[0].state = EC_STATE_PRE_OP;
//     ecx_writestate(&ecx_context, 0);
//     ecx_statecheck(&ecx_context, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
//     if (ecx_context.slavelist[0].state != EC_STATE_PRE_OP) {
//         printf("ERROR: Slaves failed to enter PRE_OP State for PDO mapping!\n");
//         goto cleanup;
//     }
//     printf("INFO: Slaves entered PRE_OP State for configuration\n");

//     if (ecx_config_map_group(&ecx_context, IOmap, 0) <= 0) {
//         printf("ERROR: PDO Mapping failed!\n");
//         goto cleanup;
//     }
    
//     int out_bytes = ecx_context.slavelist[0].Obytes;
//     int in_bytes = ecx_context.slavelist[0].Ibytes;
//     printf("INFO: PDO Mapping Success - Output: %d Bytes, Input: %d Bytes\n", out_bytes, in_bytes);

//     printf("Step 4: Initialize Motor Configurations...\n");
//     init_motor_configs();
    
//     if (motor_count == 0) {
//         printf("ERROR: No motors configured!\n");
//         goto cleanup;
//     }

//     verify_motor_pdo_mapping();
    
//     boolean all_motors_valid = TRUE;
//     for (i = 0; i < motor_count; i++) {
//         if (!validate_motor_data_access(i)) {
//             all_motors_valid = FALSE;
//             printf("ERROR: Motor %d has invalid data configuration!\n", i);
//         }
//     }
    
//     if (!all_motors_valid) {
//         printf("ERROR: Some motors have invalid configuration, exiting...\n");
//         goto cleanup;
//     }

//     printf("Step 5: Configure DC Synchronization...\n");
//     ecx_configdc(&ecx_context);
//     ecx_dcsync0(&ecx_context, 1, 1, EC_CYCLETIME, 0);
//     printf("INFO: DC Synchronization Activated - Cycle Time: %d us\n", EC_CYCLETIME_US);

//     printf("Step 6: Switch Slaves to SAFE_OP State...\n");
//     ecx_context.slavelist[0].state = EC_STATE_SAFE_OP;
//     ecx_writestate(&ecx_context, 0);
//     ecx_statecheck(&ecx_context, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
//     if (ecx_context.slavelist[0].state != EC_STATE_SAFE_OP) {
//         printf("ERROR: Slaves failed to enter SAFE_OP State!\n");
//         for (i = 1; i <= slave_count; i++) {
//             if (ecx_context.slavelist[i].state != EC_STATE_SAFE_OP) {
//                 printf("  Slave %d: State=0x%02X, ErrorCode=0x%04X\n",
//                        i, ecx_context.slavelist[i].state, ecx_context.slavelist[i].ALstatuscode);
//             }
//         }
//         goto cleanup;
//     }
//     printf("INFO: All Slaves entered SAFE_OP State!\n");

//     printf("Step 7: Switch Slaves to OPERATIONAL State...\n");
//     expectedWKC = (ecx_context.grouplist[0].outputsWKC * 2) + ecx_context.grouplist[0].inputsWKC;
//     printf("INFO: Expected Work Counter (WKC): %d\n", expectedWKC);

//     ecx_context.slavelist[0].state = EC_STATE_OPERATIONAL;
//     ecx_writestate(&ecx_context, 0);

//     chk = 200;
//     do {
//         ecx_send_processdata(&ecx_context);
//         wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);
//         ecx_statecheck(&ecx_context, 0, EC_STATE_OPERATIONAL, EC_CYCLETIME_US);
//     } while (chk-- && (ecx_context.slavelist[0].state != EC_STATE_OPERATIONAL) && !exit_flag);

//     if (ecx_context.slavelist[0].state != EC_STATE_OPERATIONAL) {
//         printf("ERROR: Slaves failed to enter OPERATIONAL State!\n");
//         goto cleanup;
//     }
    
//     inOP = TRUE;
//     for (i = 0; i < motor_count; i++) {
//         motor_configs[i].is_operational = TRUE;
//     }
    
//     printf("INFO: All Slaves entered OPERATIONAL State! Start Single Motor CSP Control...\n");

//     printf("\n===================== Single Motor CSP Control Loop Started =====================\n");
//     printf("WKC: %d | Motor 0 Status | \n", wkc);

//     int global_cycle = 0;

//     while (!exit_flag && global_cycle <= 10000) {
//         global_cycle++;

//         // 只控制motor0
//         if (motor_count > 0 && motor_configs[0].is_operational) {
//             control_single_motor(0);
//         }

//         ecx_send_processdata(&ecx_context);
//         wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);

//         if (global_cycle % 10 == 0) {
//             printf("Cycle: %4d | WKC: %d | ", global_cycle, wkc);
//             if (motor_count > 0) {
//                 print_motor_status(0);
//             }
//             printf("\r");
//             fflush(stdout);
//             needlf = TRUE;
//         }

//         osal_usleep(EC_CYCLETIME_US);
//     }

// cleanup:
//     printf("\nStep 8: Cleanup - Switch Slaves to INIT State...\n");
//     inOP = FALSE;
//     if (ecx_context.slavelist != NULL) {
//         ecx_context.slavelist[0].state = EC_STATE_INIT;
//         ecx_writestate(&ecx_context, 0);
//         ecx_statecheck(&ecx_context, 0, EC_STATE_INIT, EC_TIMEOUTSTATE * 2);
//         ecx_close(&ecx_context);
//     }
// }

// void print_usage() {
//     printf("Usage: ./single_motor_control <ethernet_adapter_name>\n");
//     printf("Example: ./single_motor_control eth0\n");
//     printf("Note: Run as root to access Ethernet adapter!\n");
//     printf("This program controls only Motor0 (Slave1)\n");
// }

// void signal_handler(int sig) {
//     if (sig == SIGINT) {
//         printf("\n\nINFO: Received Ctrl+C, preparing to exit...\n");
//         exit_flag = TRUE;
//     }
// }

#include "stdio.h"
#include "string.h"
#include "stdint.h"
#include "signal.h"
#include "unistd.h"
#include "stdlib.h"
#include "pthread.h"

// SOEM库头文件
#include "soem/soem.h"
#include "osal.h"
#include "oshw.h"
#include <inttypes.h>

// 全局变量区域
#define MAX_SLAVES 1  // 仅支持1个从站（Slave 2），减少冗余

// -------------------------- 全局配置（需根据实际硬件修改）--------------------------
#define EC_CYCLETIME     4000000  // 4ms（单位：ns），原2ms标注错误，已修正
#define EC_CYCLETIME_US  (EC_CYCLETIME / 1000)  // 转换为us：4000us

// 电机参数配置
#define MOTOR_RESOLUTION  131072  // 电机每转脉冲数
#define CIRCLE_CNT        MOTOR_RESOLUTION  // 每圈对应的位置值

// 控制模式定义
#define MODE_CSP          0x08  // Cyclic Synchronous Position模式

// 状态打印间隔（单位：ms）
#define STATUS_PRINT_INTERVAL 1000  // 缩短为1秒，更快观察Slave 2状态

// DS402状态字位定义
#define STATUSWORD_MODE_READY  (1 << 8)  // 位8：模式就绪
#define STATUSWORD_FAULT       (1 << 3)  // 位3：故障状态
#define STATUSWORD_SWITCH_ON   (1 << 0)  // 位0：开关使能
#define STATUSWORD_OP_ENABLE   (1 << 1)  // 位1：操作使能
#define STATUSWORD_MODE_CONFIRM (1 << 7) // 位7：模式确认（控制字用）
#define STATUSWORD_READY_SWITCH (1 << 2) // 位2：就绪切换

// -------------------------- 信息打印开关（1=开启打印，0=关闭打印）--------------------------
#define INFO_PRINT_ENABLE 1  // 全局开关，修改此值控制所有调试信息输出

// -------------------------- PDO映射结构体（顺序正确，适配Slave 2）--------------------------
#pragma pack(push, 1)  // 按1字节对齐

// 输出PDO结构体（主站→Slave 2）
typedef struct {
    uint16_t controlword;    // 控制字（DS402：0x6040:00）- 2字节
    int32_t  target_pos;     // 目标位置（DS402：0x607A:00）- 4字节
    int32_t  target_vel;     // 目标速度（DS402：0x60FF:00）- 4字节
    int16_t  target_torque;  // 目标扭矩（DS402：0x6071:00）- 2字节
    int16_t  max_torque;     // 最大扭矩限制（DS402：0x6072:00）- 2字节
    int8_t   mode_op;        // 控制模式（DS402：0x6060:00）- 1字节
    int8_t   reserved;       // 预留字节 - 1字节
} outputs_t;  // 总16字节（需与Slave 2的输出PDO映射长度一致）

// 输入PDO结构体（Slave 2→主站）
typedef struct {
    uint16_t statusword;     // 状态字（DS402：0x6041:00）- 2字节
    int32_t  actual_pos;     // 实际位置（DS402：0x6064:00）- 4字节
    int32_t  actual_vel;     // 实际速度（DS402：0x606C:00）- 4字节
    int16_t  actual_torque;  // 实际扭矩（DS402：0x6077:00）- 2字节
    int16_t  error_code;     // 错误码（DS402：0x603F:00）- 2字节
    int8_t   mode_op;        // 当前控制模式（DS402：0x6061:00）- 1字节
    int8_t   reserved;       // 预留字节 - 1字节
} inputs_t;  // 总16字节（需与Slave 2的输入PDO映射长度一致）

#pragma pack(pop)

// -------------------------- 电机配置结构体（仅适配Slave 2）--------------------------
typedef struct {
    int slave_index;         // 从站索引（固定为2）
    uint8* outputs;          // Slave 2输出数据指针
    uint8* inputs;           // Slave 2输入数据指针
    int Obytes;              // Slave 2输出字节数（需为16）
    int Ibytes;              // Slave 2输入字节数（需为16）
    boolean is_operational;  // Slave 2运行状态
    
    // 电机控制参数（仅Slave 2）
    int32_t target_pos;      // 目标位置
    int32_t actual_pos;      // 实际位置
    int32_t base_pos_for_increment;  // 第500周期的实际位置（后续累加基准）
    int cycle_cnt;           // 循环计数（Slave 2独立计数）
    
    // 模式配置状态跟踪
    int fault_cleared;       // 故障是否已清除（0=未清除，1=已清除）
    char name[32];           // 电机名称（固定为"Slave2-Motor"）
    int last_error_code;     // 上一次错误码（检测Slave 2错误变化）
} motor_config_t;

// -------------------------- 全局变量（仅保留Slave 2相关）--------------------------
ecx_contextt ecx_context;
uint8_t      IOmap[4096];   // PDO映射缓冲区（足够Slave 2使用）
pthread_t    thread_monitor;// 从站监控线程（仅监控Slave 2）
pthread_t    thread_status_printer; // 状态打印线程（仅打印Slave 2）

static motor_config_t motor_config;  // 单电机配置（仅Slave 2）
static int motor_count = 0;          // 电机数量（固定为1，仅Slave 2）

volatile int  wkc;                   // 工作计数器（验证Slave 2通讯）
int           expectedWKC;           // 预期工作计数器（适配Slave 2）
boolean       inOP;                  // 全局OP状态
boolean       needlf = FALSE;
uint8_t       current_group = 0;
volatile boolean exit_flag = FALSE;

// 模式就绪标记（仅Slave 2）
volatile int motor_csp_ready = 0;

// -------------------------- 函数声明（聚焦Slave 2）--------------------------
void *slave_monitor(void *ptr);      // 仅监控Slave 2状态
void *status_printer_thread(void *ptr); // 仅打印Slave 2状态
void csp_control_main(char *ifname); // 核心控制（仅Slave 2）
void print_usage();
void signal_handler(int sig);

// Slave 2控制函数
void init_slave2_config();            // 初始化Slave 2配置
void setup_slave2_control_param();    // 设置Slave 2控制参数
void control_slave2_motor();          // 单独控制Slave 2电机
void print_slave2_status();           // 打印Slave 2状态

// 诊断函数（仅Slave 2）
void detailed_slave2_pdo_analysis();  // 分析Slave 2 PDO映射
void print_all_slave_status();        // 仅打印Slave 2状态



// -------------------------- 条件打印宏（控制Slave 2日志输出）--------------------------
#if INFO_PRINT_ENABLE
#define INFO_PRINT(...) printf(__VA_ARGS__)
#else
#define INFO_PRINT(...) do {} while(0)
#endif

// -------------------------- 从站状态转换为字符串（仅用于Slave 2）--------------------------
const char* ec_state_to_string(uint16_t state) {
    switch(state) {
        case EC_STATE_INIT: return "INIT";
        case EC_STATE_PRE_OP: return "PRE_OP";
        case EC_STATE_SAFE_OP: return "SAFE_OP";
        case EC_STATE_OPERATIONAL: return "OPERATIONAL";
        case EC_STATE_SAFE_OP + EC_STATE_ERROR: return "SAFE_OP+ERROR";
        case EC_STATE_NONE: return "NONE (Lost)";
        default: return "UNKNOWN";
    }
}




// -------------------------- 主函数（仅初始化Slave 2控制）--------------------------
int main(int argc, char **argv) {
    signal(SIGINT, signal_handler);
    INFO_PRINT("===================== 单独控制 Slave 2 程序 =====================\n");

    if (argc != 2) {
        print_usage();
        // 打印可用网卡（方便用户选择）
        ec_adaptert *adapter = ec_find_adapters();
        ec_adaptert *adapter_head = adapter;
        if (adapter != NULL) {
            INFO_PRINT("\n可用以太网适配器：\n");
            while (adapter != NULL) {
                INFO_PRINT("  - %s （描述：%s）\n", adapter->name, adapter->desc);
                adapter = adapter->next;
            }
        } else {
            INFO_PRINT("\n未发现可用以太网适配器！\n");
        }
        ec_free_adapters(adapter_head);
        return -1;
    }

    // 创建监控线程（仅监控Slave 2）
    if (pthread_create(&thread_monitor, NULL, slave_monitor, NULL) != 0) {
        INFO_PRINT("ERROR: 创建Slave 2监控线程失败！\n");
        return -1;
    }

    // 创建状态打印线程（仅打印Slave 2）
    if (pthread_create(&thread_status_printer, NULL, status_printer_thread, NULL) != 0) {
        INFO_PRINT("ERROR: 创建Slave 2状态打印线程失败！\n");
        pthread_join(thread_monitor, NULL);
        return -1;
    }

    // 核心控制逻辑（仅Slave 2）
    csp_control_main(argv[1]);

    // 程序退出清理
    inOP = FALSE;
    pthread_join(thread_monitor, NULL);
    pthread_join(thread_status_printer, NULL);
    INFO_PRINT("\n===================== 程序正常退出 =====================\n");
    return 0;
}

// -------------------------- 从站监控线程（仅监控Slave 2状态）--------------------------
void *slave_monitor(void *ptr) {
    (void)ptr;
    const int TARGET_SLAVE = 2; // 固定监控Slave 2

    while (!exit_flag) {
        if (inOP && ((wkc < expectedWKC) || ecx_context.grouplist[current_group].docheckstate)) {
            if (needlf) {
                needlf = FALSE;
                INFO_PRINT("\n");
            }

            ecx_context.grouplist[current_group].docheckstate = FALSE;
            ecx_readstate(&ecx_context);

            // 仅处理Slave 2
            ec_slavet *slave = &ecx_context.slavelist[TARGET_SLAVE];
            if (slave->group != current_group) continue;

            // 检查Slave 2状态是否异常
            if (slave->state != EC_STATE_OPERATIONAL) {
                ecx_context.grouplist[current_group].docheckstate = TRUE;
                motor_config.is_operational = FALSE; // 更新Slave 2运行状态

                // 状态异常处理（针对Slave 2）
                if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) {
                    INFO_PRINT("ERROR: Slave 2 状态异常 [SAFE_OP+ERROR]！监控603F错误码...\n");
                }
                else if (slave->state == EC_STATE_SAFE_OP) {
                    INFO_PRINT("WARNING: Slave 2 处于 [SAFE_OP]！尝试切换到OP...\n");
                    slave->state = EC_STATE_OPERATIONAL;
                    ecx_writestate(&ecx_context, TARGET_SLAVE);
                }
                else if (slave->state > EC_STATE_NONE) {
                    if (ecx_reconfig_slave(&ecx_context, TARGET_SLAVE, EC_TIMEOUTRET)) {
                        slave->islost = FALSE;
                        INFO_PRINT("INFO: Slave 2 重新配置成功！\n");
                    }
                }
                else if (!slave->islost) {
                    ecx_statecheck(&ecx_context, TARGET_SLAVE, EC_STATE_OPERATIONAL, EC_TIMEOUTRET * 5);
                    if (slave->state == EC_STATE_NONE) {
                        slave->islost = TRUE;
                        INFO_PRINT("ERROR: Slave 2 丢失！（请检查接线和供电）\n");
                    }
                }
            }

            // Slave 2恢复检测
            if (slave->islost) {
                if (slave->state == EC_STATE_NONE) {
                    if (ecx_recover_slave(&ecx_context, TARGET_SLAVE, EC_TIMEOUTRET)) {
                        slave->islost = FALSE;
                        motor_config.is_operational = TRUE;
                        INFO_PRINT("INFO: Slave 2 恢复连接成功！\n");
                    }
                } else {
                    slave->islost = FALSE;
                    motor_config.is_operational = TRUE;
                    INFO_PRINT("INFO: Slave 2 重新找到！\n");
                }
            }

            // 确认Slave 2恢复OP状态
            if (!ecx_context.grouplist[current_group].docheckstate) {
                motor_config.is_operational = TRUE;
                INFO_PRINT("INFO: Slave 2 恢复 [OPERATIONAL] 状态！\n");
            }
        }

        osal_usleep(10000); // 10ms检测一次Slave 2状态
    }
    return NULL;
}

// -------------------------- 状态打印线程（仅打印Slave 2状态）--------------------------
void *status_printer_thread(void *ptr) {
    (void)ptr;
    
    while (!exit_flag) {
        osal_usleep(STATUS_PRINT_INTERVAL * 1000); // 按间隔打印
        print_all_slave_status(); // 仅打印Slave 2
    }
    return NULL;
}

// -------------------------- 全量状态打印（仅输出Slave 2信息）--------------------------
void print_all_slave_status() {
    ecx_readstate(&ecx_context);
    ecx_statecheck(&ecx_context, 2, EC_STATE_OPERATIONAL, 0); // 仅检查Slave 2
    
    INFO_PRINT("\n===================== Slave 2 状态快照 =====================\n");
    
    // 打印时间戳
    struct timeval tv;
    gettimeofday(&tv, NULL);
    uint64_t timestamp_ms = (uint64_t)tv.tv_sec * 1000 + tv.tv_usec / 1000;
    INFO_PRINT("时间戳: %lu ms\n", (unsigned long)timestamp_ms);
    
    // 打印Slave 2基础状态
    ec_slavet *slave2 = &ecx_context.slavelist[2];
    INFO_PRINT("Slave 2 基础信息：\n");
    INFO_PRINT("  名称: %s\n", slave2->name);
    INFO_PRINT("  状态: %s\n", ec_state_to_string(slave2->state));
    INFO_PRINT("  是否丢失: %s\n", slave2->islost ? "是" : "否");
    INFO_PRINT("  AL错误码: 0x%04X\n", slave2->ALstatuscode);
    INFO_PRINT("  工作计数器(WKC): %d\n", wkc);
    INFO_PRINT("-------------------------------------------------\n");
    
    // 打印Slave 2电机状态（DS402）
    inputs_t *in_ptr = (inputs_t *)motor_config.inputs;
    int mode_ready = 0, has_fault = 0;
    uint16_t error_code = 0;
    if (in_ptr) {
        mode_ready = (in_ptr->statusword & STATUSWORD_MODE_READY) ? 1 : 0;
        has_fault = (in_ptr->statusword & STATUSWORD_FAULT) ? 1 : 0;
        error_code = in_ptr->error_code;
        
        // 错误码变化时额外提示
        if (error_code != motor_config.last_error_code) {
            INFO_PRINT("ALERT: Slave 2 603F错误码变化 - 旧:0x%04X → 新:0x%04X\n",
                   motor_config.last_error_code, error_code);
            motor_config.last_error_code = error_code;
        }
    }
    
    INFO_PRINT("Slave 2 电机状态（DS402）：\n");
    INFO_PRINT("  运行状态: %s\n", motor_config.is_operational ? "正常" : "异常");
    INFO_PRINT("  故障状态: %s\n", has_fault ? "有故障" : "无故障");
    INFO_PRINT("  模式就绪: %s\n", mode_ready ? "已就绪" : "未就绪");
    INFO_PRINT("  当前模式: 0x%02X（期望:0x08=CSP）\n", in_ptr ? in_ptr->mode_op : 0xFF);
    INFO_PRINT("  目标位置: %d\n", motor_config.target_pos);
    INFO_PRINT("  实际位置: %d\n", in_ptr ? in_ptr->actual_pos : 0);
    INFO_PRINT("  状态字: 0x%04X\n", in_ptr ? in_ptr->statusword : 0xFFFF);
    INFO_PRINT("  错误码(603F): 0x%04X\n", error_code);
    INFO_PRINT("=================================================\n");
}

// -------------------------- 验证Slave 2数据访问有效性 --------------------------
boolean validate_slave2_data_access() {
    if (motor_config.outputs == NULL || motor_config.inputs == NULL) {
        INFO_PRINT("WARNING: Slave 2 PDO指针无效！\n");
        return FALSE;
    }
    
    if (motor_config.Obytes < sizeof(outputs_t) || motor_config.Ibytes < sizeof(inputs_t)) {
        INFO_PRINT("WARNING: Slave 2 PDO大小不足！\n");
        INFO_PRINT("  需求输出字节: %lu, 实际: %d\n", sizeof(outputs_t), motor_config.Obytes);
        INFO_PRINT("  需求输入字节: %lu, 实际: %d\n", sizeof(inputs_t), motor_config.Ibytes);
        return FALSE;
    }
    
    return TRUE;
}

void init_slave2_config() {
    motor_count = 0;
    const int TARGET_SLAVE = 2;

    if (TARGET_SLAVE > ecx_context.slavecount) {
        INFO_PRINT("ERROR: 主站未识别到Slave 2！当前仅发现 %d 个从站\n", ecx_context.slavecount);
        return;
    }

    ec_slavet *slave2 = &ecx_context.slavelist[TARGET_SLAVE];
    motor_config.slave_index = TARGET_SLAVE;
    motor_config.Obytes = slave2->Obytes;
    motor_config.Ibytes = slave2->Ibytes;
    motor_config.outputs = slave2->outputs;
    motor_config.inputs = slave2->inputs;
    motor_config.is_operational = FALSE;

    // 【删除错误的ecx_config_map_slave调用，依赖全局映射】
    if (motor_config.outputs == NULL || motor_config.inputs == NULL) {
        INFO_PRINT("FATAL: Slave 2 PDO指针无效，映射失败！无法控制\n");
        return;
    }

    setup_slave2_control_param();
    snprintf(motor_config.name, 32, "Slave2-Motor");

    INFO_PRINT("Slave 2 初始化成功：\n");
    INFO_PRINT("  输出缓冲区地址: 0x%lx\n", (unsigned long)motor_config.outputs);
    INFO_PRINT("  输入缓冲区地址: 0x%lx\n", (unsigned long)motor_config.inputs);
    INFO_PRINT("  输出字节数: %d（需求: %lu）\n", motor_config.Obytes, sizeof(outputs_t));
    INFO_PRINT("  输入字节数: %d（需求: %lu）\n", motor_config.Ibytes, sizeof(inputs_t));

    motor_count = 1;
    INFO_PRINT("INFO: 已配置 %d 个电机（仅Slave 2）\n", motor_count);
}


// -------------------------- 设置Slave 2控制参数 --------------------------
void setup_slave2_control_param() {
    // 初始化核心控制变量
    motor_config.target_pos = 0;
    motor_config.actual_pos = 0;
    motor_config.base_pos_for_increment = 0; // 基准位置初始化为0
    motor_config.cycle_cnt = 0;
    motor_config.fault_cleared = 0; // 故障标记初始化为未清除
    motor_config.last_error_code = 0; // 错误码初始化为0

    // （可选）通过SDO设置Slave 2最大扭矩（根据实际电机调整）
    int16_t max_torque = 1000;
    int ret = ecx_SDOwrite(&ecx_context, 2, 0x6072, 0x00, FALSE, 
                          sizeof(max_torque), &max_torque, EC_TIMEOUTRXM);
    if (ret == 0) {
        INFO_PRINT("WARNING: Slave 2 SDO写入最大扭矩（0x6072:00）失败（可能不支持）\n");
    } else {
        INFO_PRINT("INFO: Slave 2 SDO设置最大扭矩为 %d\n", max_torque);
    }
}

// -------------------------- 单独控制Slave 2电机（核心逻辑）--------------------------
void control_slave2_motor() {
    if (!validate_slave2_data_access()) {
        INFO_PRINT("WARNING: Slave 2 数据访问无效，跳过本次控制\n");
        return;
    }

    outputs_t *out_ptr = (outputs_t *)motor_config.outputs;
    inputs_t  *in_ptr = (inputs_t *)motor_config.inputs;
    motor_config.cycle_cnt++; // 递增周期计数
    int current_cycle = motor_config.cycle_cnt;

    // -------------------------- 1. 基础指令配置（强制CSP模式）--------------------------
    out_ptr->mode_op = MODE_CSP;          // 固定发送CSP模式（0x08）
    out_ptr->target_vel = 0;              // CSP模式下速度参数可忽略
    out_ptr->target_torque = 0;           // CSP模式下扭矩参数可忽略
    out_ptr->reserved = 0;                // 预留字节填0
    out_ptr->max_torque = 1000;           // 最大扭矩限制（与SDO设置一致）

    // -------------------------- 2. 状态解析（Slave 2反馈）--------------------------
    uint16_t status = in_ptr->statusword;
    uint16_t error_code = in_ptr->error_code;
    int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;
    int ready_switch = (status & STATUSWORD_READY_SWITCH) ? 1 : 0;
    int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;

    // -------------------------- 3. 目标位置计算（适配Slave 2）--------------------------
    if (in_ptr != NULL) {
        // 前500周期：目标位置跟随实际位置（让Slave 2先同步当前位置）
        if (current_cycle <= 500) {
            motor_config.target_pos = in_ptr->actual_pos;
            // 第500周期记录基准位置（后续递增的基础）
            if (current_cycle == 500) {
                motor_config.base_pos_for_increment = in_ptr->actual_pos;
                INFO_PRINT("INFO: Slave 2 第500周期，记录基准位置: %d\n", motor_config.base_pos_for_increment);
            }
        } 
        // 500周期后：目标位置 = 基准位置 + (周期数-500)*10（每周期递增10）
        else {
            motor_config.target_pos = motor_config.base_pos_for_increment + (current_cycle - 500) * 10;
        }
        motor_config.actual_pos = in_ptr->actual_pos; // 更新实际位置缓存
    } else {
        // 异常场景：无法读取实际位置，目标位置固定为0
        motor_config.target_pos = 0;
        INFO_PRINT("WARNING: Slave 2 无法读取实际位置，目标位置设为0\n");
    }
    out_ptr->target_pos = motor_config.target_pos; // 写入目标位置

    // -------------------------- 4. 控制字配置（按阶段推进，确保CSP生效）--------------------------
    // 阶段1：故障处理（仅标记，不主动复位，避免干扰Slave 2）
    if (!motor_config.fault_cleared) {
        if (has_fault) {
            out_ptr->controlword = 0x0006; // 故障时发送电源关闭（不复位）
            INFO_PRINT("INFO: Slave 2 故障状态（603F:0x%04X），发送电源关闭（0x0006），周期:%d\n", 
                   error_code, current_cycle);
        } else {
            motor_config.fault_cleared = 1; // 无故障则标记清除
            out_ptr->controlword = 0x0006; // 电源关闭（过渡状态）
            INFO_PRINT("INFO: Slave 2 无故障，标记故障已清除，周期:%d\n", current_cycle);
        }
    }
    // 阶段2：CSP模式确认（核心：发送模式确认位0x80）
    else if (!motor_csp_ready) {
        if (current_cycle < 300) {
            // 电源关闭+模式确认（确保Slave 2识别模式变更）
            out_ptr->controlword = 0x0006 | STATUSWORD_MODE_CONFIRM; // 0x0086
            if (current_cycle == 1) {
                INFO_PRINT("INFO: Slave 2 电源关闭+模式确认（0x0086），周期:%d\n", current_cycle);
            }
        } else if (current_cycle < 1800) {
            // 模式确认阶段（根据就绪状态调整）
            out_ptr->controlword = STATUSWORD_MODE_CONFIRM | (ready_switch ? 0x0004 : 0x0000); // 0x0084或0x0080
            if (current_cycle == 300) {
                INFO_PRINT("INFO: Slave 2 模式确认中（0x%04X），周期:%d\n", out_ptr->controlword, current_cycle);
            }
            // 检测Slave 2是否进入CSP模式
            if (in_ptr->mode_op == MODE_CSP) {
                motor_csp_ready = 1;
                INFO_PRINT("SUCCESS: Slave 2 进入CSP模式（0x08），周期:%d\n", current_cycle);
            }
        } else {
            // 模式配置超时，重置周期重新尝试
            motor_config.cycle_cnt = 0;
            INFO_PRINT("WARNING: Slave 2 模式配置超时，重置周期，周期:%d\n", current_cycle);
        }
    }
    // 阶段3：CSP模式就绪，发送使能指令
    else {
        // 前100周期：开关使能（0x0087=模式确认+开关使能）
        if (current_cycle < 100) {
            out_ptr->controlword = 0x0007 | STATUSWORD_MODE_CONFIRM; // 0x0087
        } 
        // 100周期后：操作使能（0x008F=模式确认+操作使能）
        else {
            out_ptr->controlword = 0x000F | STATUSWORD_MODE_CONFIRM; // 0x008F
        }
        // 打印模式就绪状态（每100周期一次）
        if (current_cycle % 100 == 0) {
            INFO_PRINT("INFO: Slave 2 CSP模式运行中，周期:%d | 目标位置:%d | 实际位置:%d\n",
                   current_cycle, motor_config.target_pos, motor_config.actual_pos);
        }
    }
}

// -------------------------- 分析Slave 2 PDO映射（确认配置是否正确）--------------------------
void detailed_slave2_pdo_analysis() {
    INFO_PRINT("\n===== Slave 2 PDO映射详细分析 =====");
    ec_slavet *slave2 = &ecx_context.slavelist[2];

    // 主站全局PDO大小
    INFO_PRINT("\n1. 主站全局PDO大小：");
    INFO_PRINT("\n   输出: %d bytes | 输入: %d bytes", 
           ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Ibytes);

    // Slave 2单独PDO大小
    INFO_PRINT("\n2. Slave 2 PDO大小：");
    INFO_PRINT("\n   输出位: %d | 输出字节: %d", slave2->Obits, slave2->Obytes);
    INFO_PRINT("\n   输入位: %d | 输入字节: %d", slave2->Ibits, slave2->Ibytes);
    INFO_PRINT("\n   需求输出字节: %lu | 需求输入字节: %lu", sizeof(outputs_t), sizeof(inputs_t));

    // Slave 2同步管理器配置（SM2=输出，SM3=输入）
    INFO_PRINT("\n3. Slave 2 同步管理器（SM）配置：");
    for (int sm = 2; sm <= 3; sm++) {
        if (slave2->SM[sm].StartAddr > 0) {
            INFO_PRINT("\n   SM%d: 起始地址=0x%04X | 长度=%d bytes | 类型=%d", 
                   sm, slave2->SM[sm].StartAddr, slave2->SM[sm].SMlength, slave2->SMtype[sm]);
        }
    }

    // PDO映射偏移检查
    INFO_PRINT("\n4. Slave 2 PDO缓冲区偏移：");
    INFO_PRINT("\n   输出偏移: %ld | 输入偏移: %ld", 
           slave2->outputs - IOmap, slave2->inputs - IOmap);
    INFO_PRINT("\n====================================\n");
}

// -------------------------- 打印Slave 2状态（简化版）--------------------------
void print_slave2_status() {
    if (!validate_slave2_data_access()) {
        INFO_PRINT("Slave2: 数据无效 | ");
        return;
    }

    inputs_t *in_ptr = (inputs_t *)motor_config.inputs;
    outputs_t *out_ptr = (outputs_t *)motor_config.outputs;
    uint16_t status = in_ptr->statusword;
    int has_fault = (status & STATUSWORD_FAULT) ? 1 : 0;
    int mode_ready = (status & STATUSWORD_MODE_READY) ? 1 : 0;

    INFO_PRINT("Slave2: 目标=%8d 实际=%8d 模式=0x%02X(F=%d,R=%d) 错误=0x%04X | ", 
           motor_config.target_pos,
           motor_config.actual_pos,
           in_ptr->mode_op,
           has_fault,
           mode_ready,
           in_ptr->error_code);
}

// -------------------------- Slave 2核心控制主逻辑（修正DC同步和EC_NO_ERROR）--------------------------
void csp_control_main(char *ifname) {
    int chk;

    INFO_PRINT("\nStep 1: 初始化EtherCAT主站（网卡：%s）...\n", ifname);
    if (!ecx_init(&ecx_context, ifname)) {
        INFO_PRINT("ERROR: 主站初始化失败！请用root权限运行，或检查网卡是否存在\n");
        goto cleanup;
    }

    INFO_PRINT("Step 2: 配置从站，搜索Slave 2...\n");
    int slave_count = ecx_config_init(&ecx_context);
    if (slave_count <= 0) {
        INFO_PRINT("ERROR: 总线上未发现任何从站！请检查Slave 2接线和供电\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: 主站共发现 %d 个从站，目标控制Slave 2\n", slave_count);

    INFO_PRINT("Step 3: 映射Slave 2的PDO...\n");
    ecx_context.manualstatechange = 1;

    ecx_context.slavelist[0].state = EC_STATE_PRE_OP;
    ecx_writestate(&ecx_context, 0);
    ecx_statecheck(&ecx_context, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
    if (ecx_context.slavelist[0].state != EC_STATE_PRE_OP) {
        INFO_PRINT("ERROR: 从站无法进入PRE_OP状态，PDO映射失败！\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: 从站已进入PRE_OP状态，开始映射PDO\n");

    if (ecx_config_map_group(&ecx_context, IOmap, 0) <= 0) {
        INFO_PRINT("ERROR: Slave 2 PDO映射失败！\n");
        goto cleanup;
    }
    INFO_PRINT("INFO: PDO映射成功！全局输出: %d Bytes | 全局输入: %d Bytes\n", 
           ecx_context.slavelist[0].Obytes, ecx_context.slavelist[0].Ibytes);

    detailed_slave2_pdo_analysis();

    INFO_PRINT("Step 4: 初始化Slave 2配置...\n");
    init_slave2_config();
    if (motor_count == 0) {
        INFO_PRINT("ERROR: Slave 2 初始化失败，无可用电机！\n");
        goto cleanup;
    }



    INFO_PRINT("Step 5: 配置Slave 2的DC同步...\n");
    ecx_configdc(&ecx_context);
    ecx_dcsync0(&ecx_context, 1, 1, EC_CYCLETIME, 0);  // 主站同步配置
    // 【删除ecx_writedcsync调用，SOEM无此函数】
    if (ecx_context.slavelist[2].hasdc) {
        INFO_PRINT("INFO: Slave 2 支持DC同步，周期: %d us\n", EC_CYCLETIME_US);
    } else {
        INFO_PRINT("WARNING: Slave 2 不支持DC同步！CSP模式无法使用\n");
        goto cleanup;
    }

    INFO_PRINT("Step 6: 将Slave 2切换到SAFE_OP状态...\n");
    ecx_context.slavelist[0].state = EC_STATE_SAFE_OP;
    ecx_writestate(&ecx_context, 0);
    chk = 200;
    do {
        ecx_statecheck(&ecx_context, 2, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
    } while (chk-- && (ecx_context.slavelist[2].state != EC_STATE_SAFE_OP) && !exit_flag);

    if (ecx_context.slavelist[2].state != EC_STATE_SAFE_OP) {
        INFO_PRINT("ERROR: Slave 2 无法进入SAFE_OP状态！当前状态: %s\n", 
               ec_state_to_string(ecx_context.slavelist[2].state));
        goto cleanup;
    }
    INFO_PRINT("INFO: Slave 2 已进入SAFE_OP状态！\n");

    INFO_PRINT("Step 7: 将Slave 2切换到OPERATIONAL状态...\n");
    expectedWKC = (ecx_context.grouplist[0].outputsWKC * 2) + ecx_context.grouplist[0].inputsWKC;
    INFO_PRINT("INFO: 预期工作计数器（WKC）: %d\n", expectedWKC);

    ecx_context.slavelist[2].state = EC_STATE_OPERATIONAL;
    ecx_writestate(&ecx_context, 2);

    chk = 200;
    do {
        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);
        ecx_statecheck(&ecx_context, 2, EC_STATE_OPERATIONAL, EC_CYCLETIME_US);
    } while (chk-- && (ecx_context.slavelist[2].state != EC_STATE_OPERATIONAL) && !exit_flag);

    if (ecx_context.slavelist[2].state == EC_STATE_OPERATIONAL) {
        INFO_PRINT("SUCCESS: Slave 2 已进入OPERATIONAL状态！开始发送CSP指令\n");
        inOP = TRUE;
        motor_config.is_operational = TRUE;
    } else {
        INFO_PRINT("WARNING: Slave 2 未进入OP状态（当前: %s），尝试强制发送指令\n", 
               ec_state_to_string(ecx_context.slavelist[2].state));
        inOP = TRUE;
    }

    INFO_PRINT("\n===================== 开始持续控制 Slave 2 =====================\n");
    int global_cycle = 0;
    while (!exit_flag) {
        global_cycle++;
        control_slave2_motor();
        ecx_send_processdata(&ecx_context);
        wkc = ecx_receive_processdata(&ecx_context, EC_TIMEOUTRET);

        if (global_cycle % 10 == 0) {
            INFO_PRINT("全局周期: %4d | WKC: %d | ", global_cycle, wkc);
            print_slave2_status();
            INFO_PRINT("\r");
            fflush(stdout);
            needlf = TRUE;
        }

        osal_usleep(EC_CYCLETIME_US);
    }

cleanup:
    INFO_PRINT("\nStep 8: 清理资源，将Slave 2切换到INIT状态...\n");
    inOP = FALSE;
    if (ecx_context.slavelist != NULL) {
        ecx_context.slavelist[2].state = EC_STATE_INIT;
        ecx_writestate(&ecx_context, 2);
        ecx_statecheck(&ecx_context, 2, EC_STATE_INIT, EC_TIMEOUTSTATE * 2);
        ecx_close(&ecx_context);
    }
}
// -------------------------- 辅助函数 --------------------------
void print_usage() {
    INFO_PRINT("用法: ./slave2_single_control <以太网适配器名称>\n");
    INFO_PRINT("示例: ./slave2_single_control eth0\n");
    INFO_PRINT("注意: 1. 需用root权限运行；2. 确保Slave 2已接线并上电；3. 仅控制Slave 2\n");
}

void signal_handler(int sig) {
    if (sig == SIGINT) {
        INFO_PRINT("\n\nINFO: 收到Ctrl+C，准备退出...\n");
        exit_flag = TRUE;
    }
}
